#!/usr/bin/python3
# coding=utf8

import rospy
import tf.transformations
from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovarianceStamped

if __name__ == '__main__':
    rospy.init_node('publish_initial_pose')
    pub_pose = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1)

    # quat = tf.transformations.quaternion_from_euler(0,0,3.1415926/2)
    # xyz = [5.0,1.0,0]

    # quat = tf.transformations.quaternion_from_euler(-0.07897492079461067,-0.006156961134275041,-0.04)
    # xyz = [-1.0904039245123944,-0.30903553995700195,7.010124077728362]
    
    quat = tf.transformations.quaternion_from_euler(0,0,0)
    xyz = [0,0,0]


    initial_pose = PoseWithCovarianceStamped()
    initial_pose.pose.pose = Pose(Point(*xyz), Quaternion(*quat))
    initial_pose.header.stamp = rospy.Time().now()
    initial_pose.header.frame_id = 'map'
    euler = tf.transformations.euler_from_quaternion(quat)
    rospy.sleep(2)
    rospy.loginfo('----------------------------------------')
    rospy.loginfo('POSE INIT !')
    rospy.loginfo('Initial Position: x={}, y={}, z={}'.format(xyz[0], xyz[1], xyz[2]))
    rospy.loginfo('Initial Orientation (euler): roll={:.3f}, pitch={:.3f}, yaw={:.3f}'.format(*euler))
    rospy.loginfo('----------------------------------------')
    pub_pose.publish(initial_pose)
